#include "mission_4_fsm.h"
#include "math.h"
#include "timeout.h"
#include "pid_app.h"
#include "user_led.h"
#include "user_beep.h"
#include "./OLED/OLED.h"
#include "./imu/imu_math.h"
#include "./imu/imu.h"
#include "./lw_pid/lw_pid.h"
#include "./lw_timer/lw_timer_callback.h"

/********************全局变量定义********************/
lwq_t mission_4_fsm_que; /*任务4状态机的消息队列*/
state_machine_t mission_4_fsm; /*任务4状态机*/

/********************静态变量定义********************/
static uint8_t line_inspection; /*巡黑线次第*/
static float marking_angle; /*标记的偏航角度*/
static uint8_t in_place_reach_num; /*方向调整到位达标次数*/
static imu_euler car_euler_angle;

/********************静态函数声明********************/
static void mission_4_idle_handle(state_machine_t* me, param_event_t* e);
static void mission_4_ready_handle(state_machine_t* me, param_event_t* e);
static void walk_straight_line_handle(state_machine_t* me, param_event_t* e);
static void line_inspection_handle(state_machine_t* me, param_event_t* e);
static void direction_adjust_handle(state_machine_t* me, param_event_t* e);

/********************全局函数定义********************/

/**
 * @brief	任务4状态机初始化
 */
void mission_4_fsm_init(void)
{
	lwq_create(&mission_4_fsm_que, QUE_MISSION_4_MSG_LEN, sizeof(param_event_t)); /*动态创建任务4消息队列*/
	mission_4_fsm.state = MISSION_4_IDLE; /*设置初始状态*/
}

/**
 * @brief	任务4状态机调度函数
 * @param	me 状态机变量
 * @param	event 事件变量
 */
void mission_4_fsm_dispatch(state_machine_t* me, param_event_t* e)
{
	switch(me->state)
	{
		case MISSION_4_IDLE:
			mission_4_idle_handle(me, e);
			break;
		
		case MISSION_4_READY:
			mission_4_ready_handle(me, e);
			break;
		
		case M4_WALK_STRAIGHT_LINE:
			walk_straight_line_handle(me, e);
			break;
		
		case M4_LINE_INSPECTION:
			line_inspection_handle(me, e);
			break;
		
		case M4_DIRECTION_ADJUST:
			direction_adjust_handle(me, e);
			break;
	}
}

/********************静态函数定义********************/

/*空闲状态事件处理*/
static void mission_4_idle_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case MISSION_4_ENTER:
		{			
			timeout_event_load(&mission_4_fsm_que, 800, STARTING_DELAY_TIMEOUT); /*挂载超时事件*/
			me->state = MISSION_4_READY; /*切换至就绪状态*/
			break;
		}
		
		case MISSION_4_EXIT:
			break;
	}
}

/*就绪状态事件处理*/
static void mission_4_ready_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case STARTING_DELAY_TIMEOUT:
		{
			line_inspection = 1; /*初始第一次巡黑线*/
			imu_get_data(&car_euler_angle, false); /*读取欧拉角*/
			marking_angle = car_euler_angle.yaw; /*获取当前偏航角*/
			pid_set_target(&angle_pid, marking_angle); /*设置角度环目标*/
			pid_set_target(&r_speed_pid, 40.0f); /*设置目标速度*/
			pid_set_target(&l_speed_pid, 40.0f);
			pid_enable(&angle_pid); /*使能角度环*/
			gray_twostate_scan_tim_start(&mission_4_fsm_que); /*开启双态灰度扫描*/
			me->state = M4_WALK_STRAIGHT_LINE; /*切换至走直线状态*/
			break;
		}			
	}
}

/*走直线状态事件处理*/
static void walk_straight_line_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case GRAY_SCALE_BLACK:
		{			
			led_set();
			beep_set();
			sound_light_tim_start(); /*执行声光提示*/
			pid_set_target(&r_speed_pid, 40.0f); /*设置目标速度*/
			pid_set_target(&l_speed_pid, 40.0f);
			pid_disable(&angle_pid); /*失能角度环*/			
			gray_scan_tim_stop(); /*关闭灰度扫描*/
			gray_multi_scan_tim_start(&mission_4_fsm_que); /*开启多态灰度扫描*/
			me->state = M4_LINE_INSPECTION; /*切换至巡线状态*/			
			break;
		}			
	}
}

/*巡线状态事件处理*/
static void line_inspection_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case GRAY_SCALE_CENTER:
		{
			
			break;
		}
		
		case GRAY_SCALE_RIGHT_1:
		{
			break;
		}
		
		case GRAY_SCALE_RIGHT_2:
		{
			break;
		}
		
		case GRAY_SCALE_RIGHT_3:
		{
			break;
		}
		
		case GRAY_SCALE_RIGHT_4:
		{
			break;
		}
		
		case GRAY_SCALE_RIGHT_5:
		{
			break;
		}
		
		case GRAY_SCALE_LEFT_1:
		{
			break;
		}
		
		case GRAY_SCALE_LEFT_2:
		{
			break;
		}
		
		case GRAY_SCALE_LEFT_3:
		{
			break;
		}
		
		case GRAY_SCALE_LEFT_4:
		{
			break;
		}
		
		case GRAY_SCALE_LEFT_5:
		{
			break;
		}
		
		case GRAY_SCALE_WHITE:
		{			
			/*如果不是最后一次巡线*/
			if(line_inspection != M4_LAST_LINE_INSPECTION)
			{				
				led_set();
				beep_set();
				sound_light_tim_start(); /*执行声光提示*/				
				gray_scan_tim_stop(); /*关闭灰度扫描*/
				pid_set_target(&r_speed_pid, 0.0f); /*设置目标速度*/
				pid_set_target(&l_speed_pid, 0.0f);									
				
				if(IS_ODD(line_inspection)) /*如果是奇数次巡线*/
				{
					marking_angle = marking_angle - 102.75f;
				}
				else
				{
					marking_angle = marking_angle + 102.75f;
				}
				
				pid_set_target(&angle_pid, marking_angle); /*设置角度环目标*/
				pid_enable(&angle_pid); /*使能角度环*/
				line_inspection++; /*标记完成了一次巡线*/
				in_place_reach_num = 0; /*到位达标次数归零*/
				timeout_event_load(&mission_4_fsm_que, 200, STEERING_IN_PLACE_CHECK); /*挂载第一次到位检查超时事件*/
				me->state = M4_DIRECTION_ADJUST; /*切换至方向调整状态*/						
			}
			else
			{
				led_set();
				beep_set();
				sound_light_tim_start(); /*执行声光提示*/
				pid_set_target(&r_speed_pid, 0.0); /*设置目标速度*/
				pid_set_target(&l_speed_pid, 0.0);				
				gray_scan_tim_stop(); /*关闭灰度扫描*/
				OLED_ShowString(24, 16, "end of run", OLED_8X16);
				me->state = MISSION_4_IDLE; /*切换至空闲状态*/
			}
			break;
		}
	}
}

/*方向调整状态事件处理*/
static void direction_adjust_handle(state_machine_t* me, param_event_t* e)
{
	switch(e->event)
	{
		case STEERING_IN_PLACE_CHECK:
		{
			float present_angle, angle_diff;
			imu_get_data(&car_euler_angle, false); /*读取欧拉角*/
			present_angle = car_euler_angle.yaw; /*获取当前偏航角*/
			angle_diff = fabs(imu_math_cal_euler_error(marking_angle, present_angle)); /*计算角度误差*/
			
			if(angle_diff < 5.0)
			{
				in_place_reach_num++;
			}
			else
			{
				in_place_reach_num = 0;
			}
			
			if(in_place_reach_num == M4_ANGLE_IN_PLACE_THRE) /*达到到位标准*/
			{
				pid_set_target(&r_speed_pid, 500.0); /*设置目标速度*/
				pid_set_target(&l_speed_pid, 500.0);
				gray_twostate_scan_tim_start(&mission_4_fsm_que); /*开启双态灰度扫描*/
				me->state = M4_WALK_STRAIGHT_LINE; /*切换至走直线状态*/				
			}
			else
			{
				timeout_event_load(&mission_4_fsm_que, 200, STEERING_IN_PLACE_CHECK); /*继续挂载到位检查*/
			}
			break;
		}
	}
}
